#include "FetchBall.h"
#include "skill/Factory.h"
#include "parammanager.h"
#include "WorldDefine.h"
#include "VisionModule.h"
#include "staticparams.h"
#include "WorldModel.h"
namespace{
    const CGeoPoint THEIR_GOAL = CGeoPoint(PARAM::Field::PITCH_LENGTH/2,0);
    bool DEBUG_SWITCH;
}
CFetchBall::CFetchBall(){
    ZSS::ZParamManager::instance()->loadParam(DEBUG_SWITCH,"Debug/Touch",false);
}
void CFetchBall::plan(const CVisionModule* pVision){
    if (pVision->getCycle() - _lastCycle > PARAM::Vision::FRAME_RATE*0.1){
    }

    const CGeoPoint target = task().player.pos;
    const bool useInter = task().player.is_specify_ctrl_method;
    const int runner = task().executor;
    auto taskFlag = task().player.flag;
    int maxacc;
    const PlayerVisionT& me = pVision->ourPlayer(runner);
    const auto mousePos = me.Pos() + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER,me.Dir());
    const MobileVisionT& ball = pVision->ball();
    const double ballVelDir = ball.Vel().dir();
    const CGeoPoint& ballPos = ball.RawPos();
    const CGeoPoint& toballPos = ball.Pos();
    const CGeoLine ballVelLine(ballPos, ballVelDir);
    const double ballVelMod = ball.Vel().mod();
    const CGeoPoint projectionPos = ballVelLine.projection(mousePos);
    const double toBallDist = (mousePos - ballPos).mod();
    const double toTargtDist = (target - ballPos).mod();
    auto d = PARAM::Math::PI;
    CGeoPoint targetPos;
    double targetDir;
    CWorldModel i;
    int time = i.InfraredOnCount(runner);
    int offtime = i.InfraredOffCount(runner);
//    if(ballVelMod < 300){
//        targetDir = (target - ballPos).dir();
//        targetPos = ballPos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER,targetDir + PARAM::Math::PI);

//    }else{
//        targetDir = useInter ? Utils::Normalize(ballVelDir + PARAM::Math::PI) : (target - mousePos).dir();
//        targetPos = projectionPos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER,targetDir + PARAM::Math::PI);
//    }

        if(abs(ballPos.x())>PARAM::Field::PITCH_LENGTH/2+90||abs(ballPos.y())>PARAM::Field::PITCH_WIDTH/2+90)
        {
            if(toballPos.x() > PARAM::Field::PITCH_LENGTH/2+90)
            {
                targetDir = 0;
            }
            else if(toballPos.x() < -PARAM::Field::PITCH_LENGTH/2-90)
            {
                targetDir = d;
            }
            else if(toballPos.y() > PARAM::Field::PITCH_WIDTH/2+90)
            {
                targetDir = d/2;
            }
            else if(toballPos.y() < -PARAM::Field::PITCH_WIDTH/2-90)
            {
                targetDir = -d/2;
            }
            else
            {
                targetDir = (toballPos - target).dir();
            }
                if(time > 20&&(toballPos - me.Pos()).mod()<140&& abs(me.Dir()-(toballPos - me.Pos()).dir())<d/6)
                {

                    //targetDir = (toballPos - target).dir();
                    targetPos = ballPos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER+150,targetDir + PARAM::Math::PI);
                    taskFlag |= PlayerStatus::NOT_AVOID_PENALTY;
                    if(toBallDist<130)
                    {
                        taskFlag |= PlayerStatus::DRIBBLING+ PlayerStatus::NOT_AVOID_PENALTY;
                    }
                    maxacc = 3000;
                }
                else
                {
                    //targetDir = (toballPos - target).dir();
                    targetPos = ballPos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER,targetDir + PARAM::Math::PI);
                    taskFlag |= PlayerStatus::SLOWLY + PlayerStatus::NOT_AVOID_PENALTY;
                    if(toBallDist < 130)
                    {
                        taskFlag |= PlayerStatus::DRIBBLING;
                    }
                    maxacc = 2000;
                }
        }
        else
        {
                if(offtime > 100)
                {
                    targetDir = (target - toballPos).dir();
                    targetPos = ballPos + Utils::Polar2Vector(200,targetDir + PARAM::Math::PI);
                    taskFlag  = PlayerStatus::DODGE_BALL+ PlayerStatus::NOT_AVOID_PENALTY;
                    maxacc = 3000;
                }
                else
                {
                    targetDir = (toballPos - me.Pos()).dir();
                    targetPos = ballPos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER + 250,targetDir + PARAM::Math::PI);
                    taskFlag  = PlayerStatus::DODGE_BALL;
                    maxacc = 5000;
                }
        }




        // add avoid ball flag
        if(toBallDist>200){
            taskFlag |= PlayerStatus::DODGE_BALL;
        }
    taskFlag|= PlayerStatus::NOT_AVOID_PENALTY + PlayerStatus::NOT_AVOID_OUR_VEHICLE + PlayerStatus::NOT_AVOID_THEIR_VEHICLE;
    TaskT newTask(task());
    newTask.player.pos = targetPos;
    newTask.player.angle = targetDir;
    newTask.player.flag = taskFlag;
    newTask.player.max_acceleration = maxacc;
    setSubTask(TaskFactoryV2::Instance()->SmartGotoPosition(newTask));

    if(DEBUG_SWITCH){
        auto endPos = ballPos + Utils::Polar2Vector(ballVelMod,ballVelDir);
        GDebugEngine::Instance()->gui_debug_line(ballPos,endPos,4);
    }

    _lastCycle = pVision->getCycle();
    CPlayerTask::plan(pVision);
}
